//
// Ephi - simulation of magnetic fields and particles
// Copyright (C) 2007 Indrek Mandre <indrek(at)mare.ee>
// For more information please see http://www.mare.ee/indrek/ephi/
//
// This program is free software; you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation; either version 2 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License along
// with this program; if not, write to the Free Software Foundation, Inc.,
// 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
//

#include "ephi.hpp"

#include <assert.h>
#include <math.h>
#include <float.h>

#ifndef INTEGRATE_BIOT_SAVART_BY
  #define INTEGRATE_BIOT_SAVART_BY (0.005)
#endif

static prec_t t_diff_ddsq = INTEGRATE_BIOT_SAVART_BY;
static prec_t small_increment = ((INTEGRATE_BIOT_SAVART_BY)/100.0);

template <class POSFUN>
struct bfield_calc
{
  prec_t my_t_diff_ddsq;

  bfield_calc (POSFUN& posfun) : posfun(posfun)
  {
    my_t_diff_ddsq = t_diff_ddsq;
    tinc = 0.25;
    t = 0;
    this->posfun.get_pos(1.0, end);

    vect3d cur_pos;
    posfun.get_pos(t, cur_pos);
    prec_t cur_len;
    while ( true )
      {
        prec_t next_t = t + tinc;
        if ( next_t > 1.0 )
            next_t = 1.0;
        vect3d next_pos;
        posfun.get_pos(next_t, next_pos);
        cur_len = (cur_pos - next_pos).length();
        if ( cur_len > small_increment )
          {
            tinc *= 2.0/3.0;
            continue;
          }
        break;
      }
    prec_t total_len = cur_len / tinc;
    prec_t n = total_len / t_diff_ddsq;
    // the idea is to even out the units, we assume uniform distribution of the posfun against t tho
    my_t_diff_ddsq *= n / ceil(n);
  }

  bool get_next(vect3d& pos, vect3d& slope)
  {
    if ( t == 1.0 ) return false;
    vect3d cur_pos;
    posfun.get_pos(t, cur_pos);
    prec_t total_len = 0;
    vect3d middle(0, 0, 0);
    prec_t n = 0;
    vect3d start_pos = cur_pos;
    while ( t != 1.0 )
      {
        if ( total_len >= my_t_diff_ddsq && (end - cur_pos).length() > 10 * small_increment )
            break;
        prec_t next_t = t + tinc;
        if ( next_t > 1.0 )
            next_t = 1.0;
        vect3d next_pos;
        posfun.get_pos(next_t, next_pos);
        prec_t cur_len = (cur_pos - next_pos).length();
        if ( cur_len > small_increment )
          {
            tinc *= 2.0/3.0;
            continue;
          }
        total_len += cur_len;
        middle += cur_pos + next_pos;

        cur_pos = next_pos;
        t = next_t;
        n = n + 2.0;
      }

    pos = middle * (1.0 / n);
    vect3d pp;
    posfun.get_pos(t, pp);
    slope = pp - start_pos;
#if 0
    slope.normalize();
    slope *= total_len;
#endif
    return true;
  }

  POSFUN& posfun;
  prec_t tinc;
  prec_t t;
  vect3d end;
};

StaticIntegrated::StaticIntegrated (prec_t currentAMPS, prec_t wire_radius, POSF *posf) :
    posf(posf), currentAMPS(currentAMPS), wr(wire_radius)
{
  vect3d pos, slope;
  bfield_calc<POSF> calc(*posf);
  while ( calc.get_next (pos, slope) )
    {
      entry e;
      e.middle = pos;
      e.slope = slope;
      e.qmod = 0.0;
      elist.push_back (e);
    }

  KmI = MAGNETIC_CONSTANT * currentAMPS;

#if defined(PREC_DOUBLE) || defined(PREC_FLOAT)
  calcmap_mem = malloc ((elist.size() + 1) * sizeof (entry));
  calcmap_begin = (entry *)((char *)calcmap_mem + (ptrdiff_t)calcmap_mem % 16);
  memcpy (calcmap_begin, &elist[0], elist.size() * sizeof (entry));
  calcmap_end = calcmap_begin + elist.size();
  std::vector<entry> tmp;
  tmp.swap (elist);
#else
  calcmap_mem = 0;
  calcmap_begin = &elist[0];
  calcmap_end = calcmap_begin + elist.size();
#endif

#ifdef PREC_DOUBLE
  sse2_consume = elist.size() - elist.size() % 2;
  sse2_data.resize (6 * sse2_consume);
  prec_t *dest = &sse2_data[0];
  for ( size_t i = 0; i < sse2_consume; i += 2 )
    {
      *dest++ = elist[i].middle.x;
      *dest++ = elist[i + 1].middle.x;
      *dest++ = elist[i].middle.y;
      *dest++ = elist[i + 1].middle.y;
      *dest++ = elist[i].middle.z;
      *dest++ = elist[i + 1].middle.z;

      *dest++ = elist[i].slope.x;
      *dest++ = elist[i + 1].slope.x;
      *dest++ = elist[i].slope.y;
      *dest++ = elist[i + 1].slope.y;
      *dest++ = elist[i].slope.z;
      *dest++ = elist[i + 1].slope.z;
    }
#elif defined(PREC_FLOAT) // PREC_FLOAT
  sse2_consume = elist.size() - elist.size() % 4;
  sse2_data.resize (6 * sse2_consume);
  prec_t *dest = &sse2_data[0];
  for ( size_t i = 0; i < sse2_consume; i += 4 )
    {
      *dest++ = elist[i + 0].middle.x;
      *dest++ = elist[i + 1].middle.x;
      *dest++ = elist[i + 2].middle.x;
      *dest++ = elist[i + 3].middle.x;
      *dest++ = elist[i + 0].middle.y;
      *dest++ = elist[i + 1].middle.y;
      *dest++ = elist[i + 2].middle.y;
      *dest++ = elist[i + 3].middle.y;
      *dest++ = elist[i + 0].middle.z;
      *dest++ = elist[i + 1].middle.z;
      *dest++ = elist[i + 2].middle.z;
      *dest++ = elist[i + 3].middle.z;

      *dest++ = elist[i + 0].slope.x;
      *dest++ = elist[i + 1].slope.x;
      *dest++ = elist[i + 2].slope.x;
      *dest++ = elist[i + 3].slope.x;
      *dest++ = elist[i + 0].slope.y;
      *dest++ = elist[i + 1].slope.y;
      *dest++ = elist[i + 2].slope.y;
      *dest++ = elist[i + 3].slope.y;
      *dest++ = elist[i + 0].slope.z;
      *dest++ = elist[i + 1].slope.z;
      *dest++ = elist[i + 2].slope.z;
      *dest++ = elist[i + 3].slope.z;
    }
#endif

  // we find the normal vector of the possible best plane of the arc
  vect3d p0, p1, p2;
  posf->get_pos (0, p0);
  posf->get_pos (0.5, p1);
  posf->get_pos (1, p2);
  vect3d v1 = p1 - p0;
  v1.normalize();
  vect3d v2 = p2 - p1;
  v2.normalize();
  plane_normal = v1 % v2;
  if ( plane_normal * plane_normal < 0.001 )
      v1.across (plane_normal);
  else
      plane_normal.normalize();
  pi_plane_d = -plane_normal * p0;

  mid.clear();
  prec_t t = 0;
  unsigned int n = 0;
  while ( t <= 1.0 )
    {
      vect3d pos;
      posf->get_pos (t, pos);
      mid += pos;
      t = t + 0.0001;
      n++;
    }
  mid = mid * (1.0 / n);

  min_d = max_d = 0;
  max_rm = 0;
  t = 0;
  while ( t <= 1.0 )
    {
      vect3d pos;
      posf->get_pos (t, pos);
      vect3d p0 = pos + wr * plane_normal;
      vect3d p1 = pos - wr * plane_normal;

      prec_t p0d = p0 * plane_normal + pi_plane_d;
      if ( p0d < min_d ) min_d = p0d;
      if ( p0d > max_d ) max_d = p0d;

      prec_t p1d = p1 * plane_normal + pi_plane_d;
      if ( p1d < min_d ) min_d = p1d;
      if ( p1d > max_d ) max_d = p1d;

      prec_t dfm = (pos - mid).magnitude();
      if ( dfm > max_rm )
          max_rm = dfm;

      t = t + 0.0001;
    }

  max_rm = prec_t_sqrt(max_rm) + wr;
  max_rm = max_rm * max_rm;

#if 0
  char b1[128], b2[128];
  printf ("plane_normal=%s, min_d=%f, max_d=%f, mid=%s, max_r=%f\n",
      plane_normal.sprint(b1), min_d, max_d, mid.sprint(b2), prec_t_sqrt(max_rm));
#endif
}

void StaticIntegrated::setCharge (prec_t qpm)
{
  this->qpm = qpm;
  for ( entry *it = calcmap_begin; it != calcmap_end; it++ )
    {
      entry& e = *it;
      e.qmod = qpm * (SPEED_OF_LIGHT * SPEED_OF_LIGHT * MAGNETIC_CONSTANT) * e.slope.length();
    }
}

#ifdef __SSE2__
#include <emmintrin.h>

#ifdef HAVE_NASM
extern "C" void addbfield_sse_double (double *posv, double *kmiv, double *begin, double *end, double *resbuf);
extern "C" void addbfield_sse_float (float *posv, float *kmiv, float *begin, float *end, float *resbuf);
#endif
#endif

void StaticIntegrated::addFields (const vect3d& pos, vect3d& bfield, vect3d& efield)
{
  vect3d posv = pos;
  vect3d resbf(0, 0, 0);
  vect3d resef(0, 0, 0);
  entry *it = calcmap_begin;

#if 0
#if defined(__SSE2__) && defined(HAVE_NASM) // optimize: http://softpixel.com/~cwright/programming/simd/sse2.php

#ifdef PREC_DOUBLE
  #if 1
  #define PRINTREG(m, str) {\
    double res[2] ATTRIBUTE_ALIGNED16; \
    _mm_store_pd (res, m); \
    printf ("%s = %s: (%f, %f)\n", #m, str, res[0], res[1]); }
  #else
  #define PRINTREG(m, str)
  #endif

  double pv[6] ATTRIBUTE_ALIGNED16 = {posv.x, posv.x, posv.y, posv.y, posv.z, posv.z};
  double kmiv[2] ATTRIBUTE_ALIGNED16 = {KmI, KmI};
  double resdata[6] ATTRIBUTE_ALIGNED16 = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};

  prec_t *src = &sse2_data[0];

  addbfield_sse_double (pv, kmiv, src, sse2_data.end(), resdata);

  resv.x += resdata[0] + resdata[1];
  resv.y += resdata[2] + resdata[3];
  resv.z += resdata[4] + resdata[5];

#if 0
  // intrinsics on GCC 4.1 ~6-7% slower than native float code
  // nasm code is ~1-2% faster than native float code ;)
  while ( src != sse2_data.end() )
    {
      __m128d x0, x1, x2, x3, x4, x5, x6, x7;

      //////////////////////// step1: load pos and middle
      // load x0; // x0 = p.x
      // load x1; // x1 = p.y
      // load x2; // x2 = p.z
      //
      // load x3; // x3 = m.x
      // load x4; // x4 = m.y
      // load x5; // x5 = m.z
      x0 = _mm_load_pd (pv);
      x1 = _mm_load_pd (pv + 2);
      x2 = _mm_load_pd (pv + 4);

      x3 = _mm_load_pd (src); src += 2;
      x4 = _mm_load_pd (src); src += 2;
      x5 = _mm_load_pd (src); src += 2;

      //////////////////////// step2: find vector r
      // x3 = x3 - x0; // x3 = m.x - p.x (r.x)
      // x4 = x4 - x1; // x4 = m.y - p.y (r.y)
      // x5 = x5 - x2; // x5 = m.z - p.z (r.z)
      x3 = _mm_sub_pd (x3, x0);
      x4 = _mm_sub_pd (x4, x1);
      x5 = _mm_sub_pd (x5, x2);

      //////////////////////// step3: find the indiviaul multiplication elements of the r vectors
      // x0 = x3; x0 = x0 * x0; // x0 = r.x * r.x
      // x1 = x4; x1 = x1 * x1; // x1 = r.y * r.y
      // x2 = x5; x2 = x2 * x2; // x2 = r.z * r.z
      x0 = _mm_mul_pd (x3, x3);
      x1 = _mm_mul_pd (x4, x4);
      x2 = _mm_mul_pd (x5, x5);

      //////////////////////// step4: add up the multipliation elements to get the dot product value
      // x0 = x0 + x1; // x0 = r.x * r.x + r.y * r.y
      // x0 = x0 + x2; // x0 = r.x * r.x + r.y * r.y + r.z * r.z
      x0 = _mm_add_pd (x0, x1);
      x0 = _mm_add_pd (x0, x2);

      //////////////////////// step5: calculate the length of r
      // x1 = x0; x1 = sqrt(x1); // x1 = |r|
      x1 = _mm_sqrt_pd (x0);

      //////////////////////// step6: get |r|^3
      // x0 = x0 * x1; // x0 = |r|^3
      x0 = _mm_mul_pd (x0, x1);

      //////////////////////// step7: load KmI
      // load x6; // x6 = (KmI, KmI)
      x6 = _mm_load_pd (kmiv);

      //////////////////////// step8: divide KmI with the |r|^3 to find the scalar
      // x6 = x6 / x0; // x6 = mod
      x6 = _mm_div_pd (x6, x0);

      //////////////////////// step9: mul modulator with the r vector
      // x3 = x3 * x6
      // x4 = x4 * x6
      // x5 = x5 * x6
      x3 = _mm_mul_pd (x3, x6);
      x4 = _mm_mul_pd (x4, x6);
      x5 = _mm_mul_pd (x5, x6);

      //////////////////////// step 10: load slope
      // load x0; // x0 = s.x
      // load x1; // x1 = s.y
      // load x2; // x2 = s.z
      x0 = _mm_load_pd (src); src += 2;
      x1 = _mm_load_pd (src); src += 2;
      x2 = _mm_load_pd (src); src += 2;

      //////////////////////// step 11: the cross product
      // x6 = x1 * x5; // x6 = s.y * r.z
      // x7 = x2 * x4; // x7 = s.z * r.y
      // x7 = x7 - x6; // x7 = s.z * r.y - s.y * r.z = R.x
      //
      // x2 = x2 * x3; // x2 = s.z * r.x
      // x5 = x5 * x0; // x5 = s.x * r.z
      // x5 = x5 - x2; // x5 = s.x * r.z - s.z * r.x = R.y
      //
      // x0 = x0 * x4; // x0 = s.x * r.y
      // x1 = x1 * x3; // x1 = s.y * r.x
      // x1 = x1 - x0; // x1 = s.y * r.x - s.x * r.y = R.z

      x6 = _mm_mul_pd (x1, x5);
      x7 = _mm_mul_pd (x2, x4);
      x7 = _mm_sub_pd (x7, x6);

      x2 = _mm_mul_pd (x2, x3);
      x5 = _mm_mul_pd (x5, x0);
      x5 = _mm_sub_pd (x5, x2);

      x0 = _mm_mul_pd (x0, x4);
      x1 = _mm_mul_pd (x1, x3);
      x1 = _mm_sub_pd (x1, x0);

      //////////////////////// step 12: getting the result
      // result.x += x7[0] + x7[1];
      // result.y += x5[0] + x5[1];
      // result.z += x1[0] + x1[1];
      _mm_store_pd (resdata, x7);
      _mm_store_pd (resdata + 2, x5);
      _mm_store_pd (resdata + 4, x1);

      resv.x += resdata[0] + resdata[1];
      resv.y += resdata[2] + resdata[3];
      resv.z += resdata[4] + resdata[5];
    }
#endif

#else
  float pv[12] ATTRIBUTE_ALIGNED16 =
      {posv.x, posv.x, posv.x, posv.x,
      posv.y, posv.y, posv.y, posv.y,
      posv.z, posv.z, posv.z, posv.z};
  float kmiv[4] ATTRIBUTE_ALIGNED16 = {KmI, KmI, KmI, KmI};
  float rd[12] ATTRIBUTE_ALIGNED16 = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};

  prec_t *src = &sse2_data[0];

  addbfield_sse_float (pv, kmiv, src, sse2_data.end(), rd);

  resv.x += rd[0] + rd[1] + rd[2] + rd[3];
  resv.y += rd[4] + rd[5] + rd[6] + rd[7];
  resv.z += rd[8] + rd[9] + rd[10] + rd[11];

#if 0
  while ( src != sse2_data.end() )
    {
      __m128 x0, x1, x2, x3, x4, x5, x6, x7;

      //////////////////////// step1: load pos and middle
      // load x0; // x0 = p.x
      // load x1; // x1 = p.y
      // load x2; // x2 = p.z
      //
      // load x3; // x3 = m.x
      // load x4; // x4 = m.y
      // load x5; // x5 = m.z
      x0 = _mm_load_ps (pv);
      x1 = _mm_load_ps (pv + 4);
      x2 = _mm_load_ps (pv + 8);

      x3 = _mm_load_ps (src); src += 4;
      x4 = _mm_load_ps (src); src += 4;
      x5 = _mm_load_ps (src); src += 4;

      //////////////////////// step2: find vector r
      // x3 = x3 - x0; // x3 = m.x - p.x (r.x)
      // x4 = x4 - x1; // x4 = m.y - p.y (r.y)
      // x5 = x5 - x2; // x5 = m.z - p.z (r.z)
      x3 = _mm_sub_ps (x3, x0);
      x4 = _mm_sub_ps (x4, x1);
      x5 = _mm_sub_ps (x5, x2);

      //////////////////////// step3: find the indiviaul multiplication elements of the r vectors
      // x0 = x3; x0 = x0 * x0; // x0 = r.x * r.x
      // x1 = x4; x1 = x1 * x1; // x1 = r.y * r.y
      // x2 = x5; x2 = x2 * x2; // x2 = r.z * r.z
      x0 = _mm_mul_ps (x3, x3);
      x1 = _mm_mul_ps (x4, x4);
      x2 = _mm_mul_ps (x5, x5);

      //////////////////////// step4: add up the multipliation elements to get the dot product value
      // x0 = x0 + x1; // x0 = r.x * r.x + r.y * r.y
      // x0 = x0 + x2; // x0 = r.x * r.x + r.y * r.y + r.z * r.z
      x0 = _mm_add_ps (x0, x1);
      x0 = _mm_add_ps (x0, x2);

      //////////////////////// step5: calculate the length of r
      // x1 = x0; x1 = sqrt(x1); // x1 = |r|
      x1 = _mm_sqrt_ps (x0);

      //////////////////////// step6: get |r|^3
      // x0 = x0 * x1; // x0 = |r|^3
      x0 = _mm_mul_ps (x0, x1);

      //////////////////////// step7: load KmI
      // load x6; // x6 = (KmI, KmI)
      x6 = _mm_load_ps (kmiv);

      //////////////////////// step8: divide KmI with the |r|^3 to find the scalar
      // x6 = x6 / x0; // x6 = mod
      x6 = _mm_div_ps (x6, x0);

      //////////////////////// step9: mul modulator with the r vector
      // x3 = x3 * x6
      // x4 = x4 * x6
      // x5 = x5 * x6
      x3 = _mm_mul_ps (x3, x6);
      x4 = _mm_mul_ps (x4, x6);
      x5 = _mm_mul_ps (x5, x6);

      //////////////////////// step 10: load slope
      // load x0; // x0 = s.x
      // load x1; // x1 = s.y
      // load x2; // x2 = s.z
      x0 = _mm_load_ps (src); src += 4;
      x1 = _mm_load_ps (src); src += 4;
      x2 = _mm_load_ps (src); src += 4;

      //////////////////////// step 11: the cross product
      // x6 = x1 * x5; // x6 = s.y * r.z
      // x7 = x2 * x4; // x7 = s.z * r.y
      // x7 = x7 - x6; // x7 = s.z * r.y - s.y * r.z = R.x
      //
      // x2 = x2 * x3; // x2 = s.z * r.x
      // x5 = x5 * x0; // x5 = s.x * r.z
      // x5 = x5 - x2; // x5 = s.x * r.z - s.z * r.x = R.y
      //
      // x0 = x0 * x4; // x0 = s.x * r.y
      // x1 = x1 * x3; // x1 = s.y * r.x
      // x1 = x1 - x0; // x1 = s.y * r.x - s.x * r.y = R.z
      x6 = _mm_mul_ps (x1, x5);
      x7 = _mm_mul_ps (x2, x4);
      x7 = _mm_sub_ps (x7, x6);

      x2 = _mm_mul_ps (x2, x3);
      x5 = _mm_mul_ps (x5, x0);
      x5 = _mm_sub_ps (x5, x2);

      x0 = _mm_mul_ps (x0, x4);
      x1 = _mm_mul_ps (x1, x3);
      x1 = _mm_sub_ps (x1, x0);

      //////////////////////// step 12: getting the result
      // result.x += x7[0] + x7[1];
      // result.y += x5[0] + x5[1];
      // result.z += x1[0] + x1[1];
      _mm_store_ps (rd, x7);
      _mm_store_ps (rd + 4, x5);
      _mm_store_ps (rd + 8, x1);

      resv.x += rd[0] + rd[1] + rd[2] + rd[3];
      resv.y += rd[4] + rd[5] + rd[6] + rd[7];
      resv.z += rd[8] + rd[9] + rd[10] + rd[11];
    }
#endif

#endif

  it = calcmap_begin + sse2_consume;
#endif
#endif

  for ( ; it != calcmap_end; it++ )
    {
      entry&e = *it;
      vect3d r = posv - e.middle;
      prec_t rr = r.x * r.x + r.y * r.y + r.z * r.z;
      prec_t rlen3 = rr * prec_t_sqrt(rr);
      prec_t modifier = KmI/rlen3;
      resbf += modifier * (e.slope % r);
      resef += (e.qmod / rlen3) * r;
    }
  bfield += resbf;
  efield += resef;
}

void StaticIntegrated::draw (Screen& screen, const Screen::color& c)
{
  prec_t t = 0.0;
  while ( t < 1.0 )
    {
      vect3d pos;
      posf->get_pos (t, pos);
      screen.set_pixel (pos, c);
      t = t + 0.0002;
    }
}

bool StaticIntegrated::inContact (const vect3d& pos)
{
  // out of bounds form the circle around
  if ( (mid - pos).magnitude() > max_rm )
      return false;
  // too far from the plane of the curve
  prec_t d = pos * plane_normal + pi_plane_d;
  if ( d < min_d || d > max_d )
      return false;
  // XXX what now?
  return false;
}

StaticIntegrated::~StaticIntegrated ()
{
  if ( calcmap_mem )
      free (calcmap_mem);
}

// parametric function for a cubic bezier curve in 3D space
struct bezier_posfun : public StaticIntegrated::POSF
{
  bezier_posfun (const vect3d& p0, const vect3d& p1, const vect3d& p2, const vect3d& p3)
  {
    cc = (p1 - p0) * 3.0;  
    cb = (p2 - p1) * 3.0 - cc;
    ca = (p3 - p0) - cc - cb;
    cd = p0;
  }

  void get_pos (prec_t t, vect3d& out)
  {
    prec_t tt = t * t;
    prec_t ttt = t * tt;
    out = ca * ttt + cb * tt + cc * t + cd;
  }

  void get_slope (prec_t t, vect3d& out)
  {
    prec_t tt = t * t;
    out = 3 * ca * tt + 2 * cb * t + cc;
  }

private:
  vect3d ca, cb, cc, cd;
};

StaticBezier::StaticBezier (prec_t currentAMPS, prec_t wire_radius,
    const vect3d& p0, const vect3d& p1, const vect3d& p2, const vect3d& p3) :
    StaticIntegrated (currentAMPS, wire_radius, new bezier_posfun (p0, p1, p2, p3)),
    p0(p0), p1(p1), p2(p2), p3(p3)
{
}

std::string StaticBezier::get_povray_script ()
{
  char buf[1024];
  sprintf (buf, "draw_cubic_bezier(<%f,%f,%f>, <%f,%f,%f>, <%f,%f,%f>, <%f,%f,%f>, %f, 0.001)\n",
      prec2double(p0.x), prec2double(p0.y), prec2double(p0.z),
      prec2double(p1.x), prec2double(p1.y), prec2double(p1.z),
      prec2double(p2.x), prec2double(p2.y), prec2double(p2.z),
      prec2double(p3.x), prec2double(p3.y), prec2double(p3.z),
      prec2double(wr)
      );
  return buf;

}

StaticBezier::~StaticBezier ()
{
}


StaticElement::~StaticElement ()
{
}

// parametric function for a circle in 3D space
struct circle_posfun : public StaticIntegrated::POSF
{
  circle_posfun (const vect3d& pos, prec_t radius, const vect3d& n) : pos(pos), normal(n)
  {
    normal.normalize();
    vect3d unit[3] = {vect3d(1.0, 0.0, 0.0), vect3d(0.0, 1.0, 0.0), vect3d(0.0, 0.0, 1.0)};
    int i;
    for ( i = 0; i < 3; i++ )
      {
        p0 = normal % unit[i];
        if ( p0 * p0 > 0.3 )
            break;
      }
    assert (i != 3);
    p0.normalize();
    p0 = pos + p0 * radius;
    p0_pc = p0 - pos;
    nXp0_pc = normal % p0_pc;

#if 0
    char b[128];
    printf ("p0=%s\n", p0.sprint(b));
    printf ("p0_pc=%s\n", p0_pc.sprint(b));
#endif
  }

  void get_pos (prec_t t, vect3d& out)
  {
    prec_t phi = t * 2 * PREC_PI;
    out = pos + cos(phi) * p0_pc + sin(phi) * nXp0_pc;
  }

  void get_slope (prec_t t, vect3d& out)
  {
    abort(); // not implemented
  }

  void get_closest (prec_t cosphi, vect3d& out1, vect3d& out2)
  {
    // we don't know which side of the circle the angle goes, so we try angle itself and 2PI-angle
    prec_t sinphi = prec_t_sqrt (1 - cosphi * cosphi);
    out1 = pos + cosphi * p0_pc + sinphi * nXp0_pc;
    out2 = pos + cosphi * p0_pc - sinphi * nXp0_pc;
  }

  vect3d pos;
  vect3d normal;
  vect3d p0;
  vect3d p0_pc;
  vect3d nXp0_pc;
};

RingBase::RingBase (prec_t currentAMPS, const vect3d& pos, prec_t radius, prec_t wire_radius, const vect3d& n) :
    current(currentAMPS), pos(pos), radius(radius), wire_radius(wire_radius), normal(n)
{
  normal.normalize();

  vect3d unit[3] = {vect3d(1.0, 0.0, 0.0), vect3d(0.0, 1.0, 0.0), vect3d(0.0, 0.0, 1.0)};
  int i;
  for ( i = 0; i < 3; i++ )
    {
      p0 = normal % unit[i];
      if ( p0 * p0 > 0.3 )
          break;
    }
  assert (i != 3);
  p0.normalize();
  p0 = pos + p0 * radius;
  p0_pc = p0 - pos;
  nXp0_pc = normal % p0_pc;

  wrwr = wire_radius * wire_radius;
  min_rr = (radius - wire_radius) * (radius - wire_radius);
  max_rr = (radius + wire_radius) * (radius + wire_radius);
  nr = (1 / (normal * normal)) * normal;
  runit = p0_pc;
  runit.normalize();
  plane_d = -(this->normal * p0);

  lr = (1 / (normal * normal)) * normal;
}

void RingBase::get_pos (prec_t t, vect3d& out)
{
  prec_t phi = t * 2 * PREC_PI;
  out = pos + cos(phi) * p0_pc + sin(phi) * nXp0_pc;
}

void RingBase::get_slope (prec_t t, vect3d& out)
{
  prec_t phi = t * 2 * PREC_PI;
  out = - sin(phi) * p0_pc + cos(phi) * nXp0_pc;
}

void RingBase::get_closest (prec_t cosphi, vect3d& out1, vect3d& out2)
{
  // we don't know which side of the circle the angle goes, so we try angle itself and 2PI-angle
  prec_t sinphi = prec_t_sqrt (1 - cosphi * cosphi);
  out1 = pos + cosphi * p0_pc + sinphi * nXp0_pc;
  out2 = pos + cosphi * p0_pc - sinphi * nXp0_pc;
}

bool RingBase::inContact (const vect3d& pos)
{
  prec_t d = pos * normal + plane_d;
  if ( d < -wire_radius || d > wire_radius )
      return false;
  vect3d R = (pos - normal * d) - this->pos;
  prec_t RR = R * R;
  if ( RR < min_rr || RR > max_rr )
      return false;
  prec_t cosphi = runit * R / prec_t_sqrt(RR);
  vect3d P1, P2;
  get_closest  (cosphi, P1, P2);
  if ( (P1 - pos).magnitude() > wrwr && (P2 - pos).magnitude() > wrwr )
      return false;
  return true;
}

std::string RingBase::get_povray_script ()
{
  char buf[1024];

  vect3d rot;
  prec_t angle;
  vect3d z(0, 0, 1);

  rot = normal % z;
  if ( rot.magnitude() > 0.01 )
    {
      rot.normalize ();
      angle = 360 * acos (normal * z) / ( 2 * PREC_PI);
    }
  else
    {
      rot = vect3d(0, 0, 1);
      angle = 0;
    }

  sprintf (buf,
      "torus { %f, %f "
      "rotate <-90, 0, 0> Axis_Rotate_Trans(<%f,%f,%f>, %f) "
      "translate<%f, %f, %f> "
      "texture {StaticRingTexture}}\n"
      , prec2double(radius), prec2double(wire_radius),
      prec2double(rot.x), prec2double(rot.y), prec2double(rot.z), -prec2double(angle),
      prec2double(pos.x), prec2double(pos.y), prec2double(pos.z));
  return buf;
}

std::string RingBase::get_gnuplot_script ()
{
  char buf[1024];
  sprintf (buf,
      "set parametric\n"
      "R=%g\n"
      "splot [t=0:1] %g+cos(t/R)*%g+sin(t/R)*%g, %g+cos(t/R)*%g+sin(t/R)*%g, %g+cos(t/R)*%g+sin(t/R)*%g with lines notitle lt 111111111\n",
      prec2double(radius),
      prec2double(pos.x), prec2double(p0_pc.x), prec2double(nXp0_pc.x),
      prec2double(pos.y), prec2double(p0_pc.y), prec2double(nXp0_pc.y),
      prec2double(pos.z), prec2double(p0_pc.z), prec2double(nXp0_pc.z)
      );
  return buf;
}

void RingBase::ringbase_draw(Screen& screen, const Screen::color& c )
{
  vect3d pos;
  prec_t t = 0.0;
  prec_t tinc = 0.0005;
  while ( t < 1.0 )
    {
      get_pos (t, pos);
      screen.set_pixel (pos, c);
      t = t + tinc;
    }
}

StaticRingIntegrated::StaticRingIntegrated (prec_t currentAMPS, const vect3d& pos, prec_t radius, prec_t wire_radius,
    const vect3d& normal) :
    RingBase (currentAMPS, pos, radius, wire_radius, normal),
    StaticIntegrated (currentAMPS, wire_radius, this)
{
}

StaticRingIntegrated::~StaticRingIntegrated ()
{
}

bool StaticRingIntegrated::inContact (const vect3d& pos)
{
  return RingBase::inContact (pos);
}

std::string StaticRingIntegrated::get_povray_script ()
{
  return RingBase::get_povray_script ();
}

StaticRing::StaticRing (prec_t currentAMPS, const vect3d& pos, prec_t radius,
    prec_t wire_radius, const vect3d& normal) :
    RingBase (currentAMPS, pos, radius, wire_radius, normal)
{
  bfact = MAGNETIC_CONSTANT * current * 2;
  efact = 0;
  R = radius;
  RR = R * R;
}

StaticRing::~StaticRing ()
{
}

void StaticRing::addFields (const vect3d& pos, vect3d& bfield, vect3d& efield)
{
  vect3d v = (pos - this->pos);
  vect3d radialn = v - normal * (v * lr);
  prec_t r = radialn.length();
  if ( r > 1e-32 )
      radialn.normalize();
  else
      r = 0;

  prec_t a = v * normal;

  prec_t m2rR = 2 * r * R;
  prec_t m4rR = 2 * m2rR;
  prec_t rr_plus_aa = r * r + a * a;
  prec_t l = rr_plus_aa + RR;
  prec_t q = l + m2rR;
  prec_t m = m4rR/q;

  prec_t cutoff_ratio = 0;
  prec_t cutoff_b_r, cutoff_e_r;
#define CUTOFF 5e-7
  if ( m < CUTOFF && m != 0 )
    {
      prec_t qa = CUTOFF;
      prec_t qb = 2*R*(CUTOFF-2);
      prec_t qc = CUTOFF*(a*a+RR);
      prec_t cor = (-qb - prec_t_sqrt(qb*qb-4*qa*qc))/(2*qa);
      cutoff_ratio = r / cor;

      // calculate the radial component at cutoff radiur 'cor'
      {
        prec_t m2rR = 2 * cor * R;
        prec_t m4rR = 2 * m2rR;
        prec_t rr_plus_aa = cor * cor + a * a;
        prec_t l = rr_plus_aa + RR;
        prec_t q = l + m2rR;
        prec_t m = m4rR/q;
        prec_t sqrt_q = prec_t_sqrt(q);

        prec_t fk = ellipse1 (m);
        prec_t ek = ellipse2 (m, fk);

        prec_t minv = 1.0 - m;

        prec_t ef_low = q * sqrt_q * minv;

        cutoff_b_r = bfact * a * (l * (ek - fk) + m2rR * fk)/((l - m2rR) * cor * sqrt_q);
        cutoff_e_r = efact * (2 * R * fk * minv - ek * (2 * R - m * (cor + R))) / (m * ef_low);
      }
    }
  prec_t sqrt_q = prec_t_sqrt(q);
  prec_t minv = 1.0 - m;

  prec_t ef_low = q * sqrt_q * minv;

  prec_t fk = ellipse1 (m);
  prec_t ek = ellipse2 (m, fk);

  //printf ("m=%g, fk=%g, ek=%g\n", prec2double(m), prec2double(fk), prec2double(ek)); // XXX

  prec_t ek_blow = ek / (q - m4rR);

  prec_t b_a = bfact * (ek_blow * (RR - rr_plus_aa) + fk) / sqrt_q;
  prec_t b_r;
  if ( cutoff_ratio != 0 )
      b_r = cutoff_ratio * cutoff_b_r;
  else
      b_r = r == 0 ? 0 : bfact * a * (l * (ek - fk) + m2rR * fk)/((l - m2rR) * r * sqrt_q);

  prec_t e_a = efact * a * ek / ef_low;
  prec_t e_r;
  if ( cutoff_ratio != 0 )
      e_r = cutoff_ratio * cutoff_e_r;
  else
      e_r = r == 0 ? 0 : efact * (2 * R * fk * minv - ek * (2 * R - m * (r + R))) / (m * ef_low);

  bfield += b_a * normal + b_r * radialn;
  efield += e_a * normal + e_r * radialn;
}

void StaticRing::draw(Screen& screen, const Screen::color& c )
{
  ringbase_draw (screen, c);
}

bool StaticRing::inContact (const vect3d& pos)
{
  return RingBase::inContact (pos);
}

std::string StaticRing::get_povray_script ()
{
  return RingBase::get_povray_script ();
}

std::string StaticRing::get_gnuplot_script ()
{
  return RingBase::get_gnuplot_script ();
}

void StaticRing::setCharge (prec_t qpm)
{
  charge = PREC_PI * radius * 2 * qpm;
  efact = charge * (SPEED_OF_LIGHT*SPEED_OF_LIGHT*MAGNETIC_CONSTANT) * 2 / PREC_PI;
}

StaticLineBase::StaticLineBase (const vect3d& p0, const vect3d& p1, prec_t radius)
    : p0(p0), p1(p1), radius(radius)
{
  l = p1 - p0;
  l_len = l.length();
  lr = (1 / (l * l)) * l;
  mid = (p0 + p1) * 0.5;
  rr = radius * radius;
  dfms = (p0 - mid).magnitude();
  ln = l.normal();
}

bool StaticLineBase::inContact (const vect3d& pos)
{
  vect3d r0 = p0 - pos;
  vect3d R = r0 - l * (r0 * lr);
  if ( R * R > rr )
      return false;
  if ( (p0 - pos).magnitude() < rr || (p1 - pos).magnitude() < rr )
      return true;
  if ( (mid - pos).magnitude() > dfms )
      return false;
  else
      return true;
}

void StaticLineBase::draw(Screen& screen, const Screen::color& c)
{
  prec_t t = 0;
  vect3d mv = p1 - p0;
  while ( t < 1.0 )
    {
      vect3d pos = p0 + t * mv;
      screen.set_pixel (pos, c);
      t = t + 0.001;
    }
}

std::string StaticLineBase::get_povray_script ()
{
  char buf[1024];
  sprintf (buf, "cylinder { <%f, %f, %f>, <%f, %f, %f>, %f texture {StaticLineTexture}}\n",
      prec2double(p0.x), prec2double(p0.y), prec2double(p0.z),
      prec2double(p1.x), prec2double(p1.y), prec2double(p1.z),
      prec2double(radius));
  return buf;
}


StaticLine::StaticLine (prec_t currentAMPS, const vect3d& p0, const vect3d& p1, prec_t radius, prec_t rho) :
    StaticLineBase (p0, p1, radius),
    currentAMPS(currentAMPS),
    rho (rho)
{
  KmI = MAGNETIC_CONSTANT * currentAMPS;
}

void StaticLine::addFields (const vect3d& pos, vect3d& bfield, vect3d& efield)
{
  // 4 divs and 4 sqrt-s. ugh!
  vect3d r0 = p0 - pos;
  vect3d r1 = p1 - pos;
  prec_t r0len = r0.length();
  prec_t cos_phi0 = (r0 * l) / (l_len * r0len);
  prec_t r1len = r1.length();
  prec_t cos_phi1 = (r1 * l) / (l_len * r1len);
  prec_t R = (r0 - l * (r0 * lr)).length();
  prec_t mphi = cos_phi1 - cos_phi0;
  if ( !(fabs(mphi) < 1e-14 || R < 1e-8) ) // XXX we can do better than this
    {
      prec_t mod = KmI * mphi / R;
      vect3d u = r0 % l;
      u.normalize();
      //pos.print ("pos");
      //(mod * u).print ("original:");
      bfield += mod * u;
    }

  if ( rho != 0 )
    {
      vect3d r0 = pos - p0;
      vect3d r1 = pos - p1;
      prec_t a = ln * r0;
      prec_t b = -1 * (ln * r1);
      prec_t r0len = r0.length();
      prec_t r1len = r1.length();
      vect3d r0n = r0 / r0len;
      vect3d r1n = r1 / r1len;

      vect3d ef;
      if ( a > b )
        {
          ef = (r0n + ln) / (a + r0len) - (r1n + ln) / (-b + r1len);
        }
      else
        {
          ef = (r1n - ln) / (b + r1len) - (r0n - ln) / (-a + r0len);
        }
      efield -= ef;
    }
}

void StaticLine::addPotential (const vect3d& pos, prec_t& potential)
{
  vect3d r0 = pos - p0;
  vect3d r1 = pos - p1;
  prec_t a = ln * r0;
  prec_t b = -1 * (ln * r1);
  prec_t r0len = r0.length();
  prec_t r1len = r1.length();
  prec_t higher, lower;
  if ( a > b )
    {
      lower = -b + r1len;
      higher = a + r0len;
    }
  else
    {
      lower = -a + r0len;
      higher = (b + r1len);
    }
  prec_t f = higher / lower;
  potential += rho * prec_t_log (f);
}

StaticLineInfinite::StaticLineInfinite (prec_t currentAMPS, const vect3d& p0, const vect3d& dir, prec_t radius) :
    currentAMPS(currentAMPS), p0(p0), normal(dir), radius(radius)
{
  normal.normalize();
  lnormal = (1 / (normal * normal)) * normal;
  KmI2 = 2 * MAGNETIC_CONSTANT * currentAMPS;
  rr = radius * radius;
}

void StaticLineInfinite::addFields (const vect3d& pos, vect3d& bfield, vect3d& efield)
{
  // 4 divs and 4 sqrt-s. ugh!
  vect3d r0 = p0 - pos;
  vect3d R = r0 - normal * (r0 * lnormal);
  prec_t RLEN = R.length();
  R /= RLEN;
//  printf ("%e\n", RLEN);
  prec_t mod = KmI2 / RLEN;
  vect3d u = R % normal;
  u.normalize();
  bfield += mod * u;
}

void StaticLineInfinite::draw(Screen& screen, const Screen::color& c)
{
  Ephi::debug (Ephi::DEBUG_INFO, "StaticLineInfinite::draw: not implemented\n"); // XXX
}

std::string StaticLineInfinite::get_povray_script ()
{
  char buf[1024];
  vect3d ps = p0 - 1000000 * normal;
  vect3d pe = p0 + 1000000 * normal;
  sprintf (buf, "cylinder { <%f, %f, %f>, <%f, %f, %f>, %f texture {StaticLineTexture}}\n",
      prec2double(ps.x), prec2double(ps.y), prec2double(ps.z),
      prec2double(pe.x), prec2double(pe.y), prec2double(pe.z),
      prec2double(radius));
  return buf;
}

bool StaticLineInfinite::inContact (const vect3d& pos)
{
  vect3d r0 = p0 - pos;
  vect3d R = r0 - normal * (r0 * lnormal);
  return R * R <= rr;
}

StaticRectangle::StaticRectangle(prec_t currentAMPS, const vect3d& pos, const vect3d& right, const vect3d& up, prec_t wr)
{
  p1 = pos - right - up;
  p2 = pos - right + up;
  p3 = pos + right + up;
  p4 = pos + right - up;
  this->wr = wr;
  l1 = new StaticLine (currentAMPS, p1, p2, wr);
  l2 = new StaticLine (currentAMPS, p2, p3, wr);
  l3 = new StaticLine (currentAMPS, p3, p4, wr);
  l4 = new StaticLine (currentAMPS, p4, p1, wr);
}

StaticRectangle::~StaticRectangle ()
{
  delete l1;
  delete l2;
  delete l3;
  delete l4;
}

void StaticRectangle::addFields (const vect3d& pos, vect3d& bfield, vect3d& efield)
{
  l1->addFields (pos, bfield, efield);
  l2->addFields (pos, bfield, efield);
  l3->addFields (pos, bfield, efield);
  l4->addFields (pos, bfield, efield);
}

void StaticRectangle::draw(Screen& screen, const Screen::color& c)
{
  l1->draw (screen, c);
  l2->draw (screen, c);
  l3->draw (screen, c);
  l4->draw (screen, c);
}

std::string StaticRectangle::make_povsegment (const vect3d& n1, prec_t d1, const vect3d& n2, prec_t d2,
    const vect3d& p1, const vect3d& p2)
{
  char buf[1024];
  sprintf (buf, "intersection {\n"
      "  union {\n"
      "     cylinder { <%f, %f, %f>, <%f, %f, %f>, %f texture {StaticLineTexture} }\n"
      "     sphere { <%f, %f, %f>, %f texture {StaticLineTexture} }\n"
      "     sphere { <%f, %f, %f>, %f texture {StaticLineTexture} }\n"
      "  }\n"
      "  plane { <%f, %f, %f>, %f texture {StaticLineTexture} }\n"
      "  plane { <%f, %f, %f>, %f texture {StaticLineTexture} }\n"
      "}\n",
      prec2double(p1.x), prec2double(p1.y), prec2double(p1.z),
      prec2double(p2.x), prec2double(p2.y), prec2double(p2.z),
      prec2double(wr),
      prec2double(p1.x), prec2double(p1.y), prec2double(p1.z), prec2double(wr),
      prec2double(p2.x), prec2double(p2.y), prec2double(p2.z), prec2double(wr),
      prec2double(n1.x), prec2double(n1.y), prec2double(n1.z),
      prec2double(d1),
      prec2double(n2.x), prec2double(n2.y), prec2double(n2.z),
      prec2double(d2)
      );
  return buf;
}

std::string StaticRectangle::get_povray_script ()
{
  std::string ret;

  vect3d n = ((p1 - p2) % (p2 - p3)).normal();
  vect3d p1n = (((p1 - p4).normal() + (p1 - p2).normal()) % n).normal();
  vect3d p2n = (((p2 - p1).normal() + (p2 - p3).normal()) % n).normal();
  vect3d p3n = (((p3 - p2).normal() + (p3 - p4).normal()) % n).normal();
  vect3d p4n = (((p4 - p3).normal() + (p4 - p1).normal()) % n).normal();
  prec_t p1dist = -p1 * p1n;
  prec_t p2dist = -p2 * p2n;
  prec_t p3dist = -p3 * p3n;
  prec_t p4dist = -p4 * p4n;

  ret += make_povsegment(p1n, p1dist, -p2n, -p2dist, p1, p2);
  ret += make_povsegment(p2n, p2dist, -p3n, -p3dist, p2, p3);
  ret += make_povsegment(p3n, p3dist, -p4n, -p4dist, p3, p4);
  ret += make_povsegment(p4n, p4dist, -p1n, -p1dist, p4, p1);

  return ret;
}

bool StaticRectangle::inContact (const vect3d& pos)
{
  return l1->inContact(pos) ||
      l2->inContact(pos) ||
      l3->inContact(pos) ||
      l4->inContact(pos);
}

StaticCIBase::StaticCIBase (const CoilData& cd, const vect3d& p, const vect3d& n, prec_t realI, prec_t realqpm) : data(cd), pos(p), normal(n), realI(realI)
{
  realQ = realqpm / COILGEN_QMOD;
  normal.normalize();
  lr = (1 / (normal * normal)) * normal;
}

StaticCIBase::~StaticCIBase ()
{
}

void StaticCIBase::draw(Screen& screen, const Screen::color& c)
{
  if ( data.coilpair_distance == 0 )
    {
      RingBase rb(realI, pos, data.coil_radius, data.coil_wr, normal);
      rb.ringbase_draw (screen, c);
    }
  else
    {
      RingBase rb1(realI, pos + 0.5 * data.coilpair_distance * normal, data.coil_radius, data.coil_wr, -normal);
      RingBase rb2(realI, pos - 0.5 * data.coilpair_distance * normal, data.coil_radius, data.coil_wr, normal);
      rb1.ringbase_draw (screen, c);
      rb2.ringbase_draw (screen, c);
    }
}

std::string StaticCIBase::get_povray_script ()
{
  if ( data.coilpair_distance == 0 )
    {
      RingBase rb(realI, pos, data.coil_radius, data.coil_wr, normal);
      return rb.get_povray_script();
    }
  else
    {
      RingBase rb1(realI, pos + 0.5 * data.coilpair_distance * normal, data.coil_radius, data.coil_wr, -normal);
      RingBase rb2(realI, pos - 0.5 * data.coilpair_distance * normal, data.coil_radius, data.coil_wr, normal);
      return rb1.get_povray_script() + rb2.get_povray_script();
    }
}

bool StaticCIBase::inContact (const vect3d& pos)
{
  if ( !data.cmap ) return false;
  vect3d v = (pos - this->pos);
  vect3d radialn = v - normal * (v * lr);
  prec_t r = radialn.length();
  if ( r <= 1e-32 )
      r = 0;
  prec_t a = v * normal;
  prec_t eff_a;
  if ( data.has_axial_add )
    {
      eff_a = a + data.geom_axial_add;
      if ( eff_a < 0.0 ) return false;
    }
  else
    {
      eff_a = fabs(a);
    }
  if ( r > data.geom_radial || eff_a > data.geom_axial )
      return false;
  size_t ri = COILGEN_BUFFER + (size_t)lround(r*data.rec_cr);
  size_t ai = COILGEN_BUFFER + (size_t)lround(eff_a*data.rec_ca);
  size_t index = ri * data.axial_samples + ai;
  return (data.cmap[index >> 3] & (1 << (index & 7))) != 0;
}

StaticCINN::StaticCINN (const CoilData& cd, const vect3d& p, const vect3d& n, prec_t realI, prec_t realqpm)
  : StaticCIBase (cd, p, n, realI, realqpm)
{
}

StaticCINN::~StaticCINN ()
{
}

void StaticCINN::addFields (const vect3d& pos, vect3d& bfield, vect3d& efield)
{
  vect3d v = (pos - this->pos);
  vect3d radialn = v - normal * (v * lr);
  prec_t r = radialn.length();
  if ( r > 1e-32 )
    {
      radialn /= r;
    }
  else
    {
      r = 0;
      radialn.clear();
    }
  prec_t a = v * normal;
  prec_t eff_a;
  prec_t emod = 1.0;
  prec_t bamod = 1.0;
  prec_t brmod = 1.0;
  if ( data.has_axial_add )
    {
      eff_a = a + data.geom_axial_add;
      if ( eff_a < 0.0 || r > data.geom_radial || eff_a > data.geom_axial )
          return;
    }
  else
    {
      if ( a < 0 )
        {
          emod = -1.0;
          if ( data.mirror )
              bamod = -1.0;
          else
              brmod = -1.0;
        }
      eff_a = fabs(a);
      if ( r > data.geom_radial || eff_a > data.geom_axial )
          return;
    }

  size_t ri = COILGEN_BUFFER + (size_t)lround(r * data.rec_cr);
  size_t ai = COILGEN_BUFFER + (size_t)lround(eff_a * data.rec_ca);
  size_t index = ri * data.axial_samples + ai;
  if ( data.bmapemap )
    {
      bfield += radialn * (realI * brmod * data.bmapemap[index].rb) + normal * (realI * bamod * data.bmapemap[index].ab);
      efield += radialn * (realQ * data.bmapemap[index].re) + normal * (realQ * emod * data.bmapemap[index].ae);
    }
  else if ( data.bmap )
    {
      bfield += radialn * (realI * brmod * data.bmap[index].rb) + normal * (realI * bamod * data.bmap[index].ab);
    }
}

StaticCIL::StaticCIL (const CoilData& cd, const vect3d& p, const vect3d& n, prec_t realI, prec_t realqpm)
  : StaticCIBase (cd, p, n, realI, realqpm)
{
}

StaticCIL::~StaticCIL ()
{
}

void StaticCIL::addFields (const vect3d& pos, vect3d& bfield, vect3d& efield)
{
  vect3d v = (pos - this->pos);
  vect3d radialn = v - normal * (v * lr);
  prec_t r = radialn.length();
  if ( r > 1e-32 )
    {
      radialn /= r;
    }
  else
    {
      r = 0;
      radialn.clear();
    }
  prec_t a = v * normal;
  prec_t eff_a;
  prec_t emod = 1.0;
  prec_t bamod = 1.0;
  prec_t brmod = 1.0;
  if ( data.has_axial_add )
    {
      eff_a = a + data.geom_axial_add;
      if ( eff_a < 0.0 || r > data.geom_radial || eff_a > data.geom_axial )
          return;
    }
  else
    {
      if ( a < 0 )
        {
          emod = -1.0;
          if ( data.mirror )
              bamod = -1.0;
          else
              brmod = -1.0;
        }
      eff_a = fabs(a);
      if ( r > data.geom_radial || eff_a > data.geom_axial )
          return;
    }

  prec_t allrib = r * data.rec_cr;
  prec_t allaib = eff_a * data.rec_ca;
  prec_t rib = floor (allrib);
  prec_t aib = floor (allaib);
  size_t ri = COILGEN_BUFFER + prec2size_t(rib);
  size_t ai = COILGEN_BUFFER + prec2size_t(aib);
  size_t index = ri * data.axial_samples + ai;

  prec_t x = allaib - aib;
  prec_t y = allrib - rib;
  prec_t xm1 = 1.0 - x;
  prec_t ym1 = 1.0 - y;

  prec_t xm1ym1 = xm1 * ym1;
  prec_t xym1 = x * ym1;
  prec_t xm1y = xm1 * y;
  prec_t xy = x * y;

  if ( data.bmapemap )
    {
      size_t axial_samples = data.axial_samples;
      coilgen_bmapemap *bmapemap = data.bmapemap;

      prec_t brcomp = bmapemap[index].rb * xm1ym1;
      prec_t bacomp = bmapemap[index].ab * xm1ym1;
      prec_t ercomp = bmapemap[index].re * xm1ym1;
      prec_t eacomp = bmapemap[index].ae * xm1ym1;

      brcomp += bmapemap[index + 1].rb * xym1;
      bacomp += bmapemap[index + 1].ab * xym1;
      ercomp += bmapemap[index + 1].re * xym1;
      eacomp += bmapemap[index + 1].ae * xym1;

      brcomp += bmapemap[index + axial_samples].rb * xm1y;
      bacomp += bmapemap[index + axial_samples].ab * xm1y;
      ercomp += bmapemap[index + axial_samples].re * xm1y;
      eacomp += bmapemap[index + axial_samples].ae * xm1y;

      brcomp += bmapemap[index + axial_samples + 1].rb * xy;
      bacomp += bmapemap[index + axial_samples + 1].ab * xy;
      ercomp += bmapemap[index + axial_samples + 1].re * xy;
      eacomp += bmapemap[index + axial_samples + 1].ae * xy;

      bfield += (realI * brmod * brcomp) * radialn + (realI * bamod * bacomp) * normal;
      efield += (realQ * ercomp) * radialn + (realQ * emod * eacomp) * normal;
    }
  else if ( data.bmap )
    {
      prec_t rcomp =
          data.bmap[index].rb * xm1ym1 +
          data.bmap[index + 1].rb * xym1 +
          data.bmap[index + data.axial_samples].rb * xm1y +
          data.bmap[index + data.axial_samples + 1].rb * xy;
      prec_t acomp =
          data.bmap[index].ab * xm1ym1 +
          data.bmap[index + 1].ab * xym1 +
          data.bmap[index + data.axial_samples].ab * xm1y +
          data.bmap[index + data.axial_samples + 1].ab * xy;

      bfield += (realI * brmod * rcomp) * radialn + (realI * bamod * acomp) * normal;
    }
}

StaticCICU::StaticCICU (const CoilData& cd, const vect3d& p, const vect3d& n, prec_t realI, prec_t realqpm)
  : StaticCIBase (cd, p, n, realI, realqpm)
{
}

StaticCICU::~StaticCICU ()
{
}

void StaticCICU::addFields (const vect3d& pos, vect3d& bfield, vect3d& efield)
{
  vect3d v = (pos - this->pos);
  vect3d radialn = v - normal * (v * lr);
  prec_t r = radialn.length();
  if ( r > 1e-32 )
    {
      radialn /= r;
    }
  else
    {
      r = 0;
      radialn.clear();
    }
  prec_t a = v * normal;
  prec_t eff_a;
  prec_t emod = 1.0;
  prec_t bamod = 1.0;
  prec_t brmod = 1.0;
  if ( data.has_axial_add )
    {
      eff_a = a + data.geom_axial_add;
      if ( eff_a < 0.0 || r > data.geom_radial || eff_a > data.geom_axial )
          return;
    }
  else
    {
      if ( a < 0 )
        {
          emod = -1.0;
          if ( data.mirror )
              bamod = -1.0;
          else
              brmod = -1.0;
        }
      eff_a = fabs(a);
      if ( r > data.geom_radial || eff_a > data.geom_axial )
          return;
    }

  prec_t allrib = r * data.rec_cr;
  prec_t allaib = eff_a * data.rec_ca;
  prec_t rib = floor (allrib);
  prec_t aib = floor (allaib);
  size_t ri = COILGEN_BUFFER + prec2size_t(rib);
  size_t ai = COILGEN_BUFFER + prec2size_t(aib);
  size_t index = ri * (data.axial_samples - 1) + ai;

  prec_t x = allaib - aib;
  prec_t y = allrib - rib;

  // now x and y are the coordinates in the range [0,1)

  if ( data.cubic_bmapemap )
    {
      prec_t brcomp, bacomp, ercomp, eacomp;
      brcomp = bacomp = ercomp = eacomp = 0;

      coilgen_cubic_bmap::matrix_t& rb = data.cubic_bmapemap[index].rb;
      coilgen_cubic_bmap::matrix_t& ab = data.cubic_bmapemap[index].ab;
      coilgen_cubic_bmap::matrix_t& re = data.cubic_bmapemap[index].re;
      coilgen_cubic_bmap::matrix_t& ae = data.cubic_bmapemap[index].ae;
      for ( int i = 3; i >= 0; i-- )
        {
          brcomp = x * brcomp + (((prec_t)rb[i][3] * y + (prec_t)rb[i][2]) * y + (prec_t)rb[i][1]) * y + (prec_t)rb[i][0];
          bacomp = x * bacomp + (((prec_t)ab[i][3] * y + (prec_t)ab[i][2]) * y + (prec_t)ab[i][1]) * y + (prec_t)ab[i][0];
          ercomp = x * ercomp + (((prec_t)re[i][3] * y + (prec_t)re[i][2]) * y + (prec_t)re[i][1]) * y + (prec_t)re[i][0];
          eacomp = x * eacomp + (((prec_t)ae[i][3] * y + (prec_t)ae[i][2]) * y + (prec_t)ae[i][1]) * y + (prec_t)ae[i][0];
        }

      bfield += (realI * brmod * brcomp) * radialn + (realI * bamod * bacomp) * normal;
      efield += (realQ * ercomp) * radialn + (realQ * emod * eacomp) * normal;
    }
  else if ( data.cubic_bmap )
    {
      prec_t rcomp = 0, acomp = 0;
      coilgen_cubic_bmap::matrix_t& rb = data.cubic_bmap[index].rb;
      coilgen_cubic_bmap::matrix_t& ab = data.cubic_bmap[index].ab;
      for ( int i = 3; i >= 0; i-- )
        {
          rcomp = x * rcomp + (((prec_t)rb[i][3] * y + (prec_t)rb[i][2]) * y + (prec_t)rb[i][1]) * y + (prec_t)rb[i][0];
          acomp = x * acomp + (((prec_t)ab[i][3] * y + (prec_t)ab[i][2]) * y + (prec_t)ab[i][1]) * y + (prec_t)ab[i][0];
        }

      bfield += (realI * brmod * rcomp) * radialn + (realI * bamod * acomp) * normal;
    }
}

StaticLineCharge::StaticLineCharge (const vect3d& p0, const vect3d& p1, prec_t rho0, prec_t rho1, prec_t blotradius)
    : StaticLineBase (p0, p1, blotradius == 0 ? (p1 - p0).length() * 0.01 : blotradius), rho0(rho0), rho1(rho1)
{
  rhodiff = rho1 - rho0;
  mod = SPEED_OF_LIGHT * SPEED_OF_LIGHT * MAGNETIC_CONSTANT * l_len;
  rhodiff_by_llen = rhodiff / l_len;
}

void StaticLineCharge::addPotential (const vect3d& pos, prec_t& potential)
{
  vect3d r0, r1;
  prec_t a, b;
  prec_t u, v;

  vect3d tv = pos - p0;
  prec_t tm = ln * tv;
  if ( tm <= 0 )
    {
      r0 = tv;
      r1 = pos - p1;
      a = -tm;
      b = -(ln * r1);
      u = rho0 - a * rhodiff_by_llen;
      v = rhodiff_by_llen;
    }
  else
    {
      r0 = pos - p1;
      r1 = tv;
      a = (ln * r0);
      b = tm;
      u = rho1 - a * -rhodiff_by_llen;
      v = -rhodiff_by_llen;
    }
  prec_t r0len = r0.length();
  prec_t r1len = r1.length();
  prec_t add = (SPEED_OF_LIGHT * SPEED_OF_LIGHT * MAGNETIC_CONSTANT) *
      (u * prec_t_log ((b + r1len) / (a + r0len)) + v * (r1len - r0len));
  potential += add;
}

void StaticLineCharge::addFields (const vect3d& pos, vect3d& bfield, vect3d& efield)
{
  vect3d r0, r1;
  prec_t a, b;
  prec_t u, v;
  vect3d n;

  vect3d tv = pos - p0;
  prec_t tm = ln * tv;
  if ( tm <= 0 )
    {
      r0 = tv;
      r1 = pos - p1;
      a = -tm;
      b = -(ln * r1);
      u = rho0 - a * rhodiff_by_llen;
      v = rhodiff_by_llen;
      n = ln;
    }
  else
    {
      r0 = pos - p1;
      r1 = tv;
      a = (ln * r0);
      b = tm;
      u = rho1 - a * -rhodiff_by_llen;
      v = -rhodiff_by_llen;
      n = -ln;
    }
  prec_t r0len = r0.length();
  prec_t r1len = r1.length();
  prec_t af = a + r0len;
  prec_t bf = b + r1len;
  prec_t lf = log(bf / af);
  vect3d r0n = r0 / r0len;
  vect3d r1n = r1 / r1len;
  vect3d add = v * (lf * n + r1n - r0n) + u * ((r1n - n) / bf - (r0n - n) / af);
  efield += -(SPEED_OF_LIGHT * SPEED_OF_LIGHT * MAGNETIC_CONSTANT) * add;
}

StaticChargedSphere::StaticChargedSphere (const vect3d& pos, prec_t radius, prec_t q) : pos(pos), radius(radius), q(q)
{
  qmod = q * (SPEED_OF_LIGHT * SPEED_OF_LIGHT * MAGNETIC_CONSTANT);
  radiusradius = radius * radius;
}

StaticChargedSphere::~StaticChargedSphere ()
{
}

void StaticChargedSphere::addFields (const vect3d& pos, vect3d& bfield, vect3d& efield)
{
  vect3d r = pos - this->pos;
  prec_t rr = r * r;
  if ( rr > radiusradius )
      efield += (qmod / (rr * prec_t_sqrt(rr))) * r;
}

void StaticChargedSphere::addPotential (const vect3d& pos, prec_t& potential)
{
  prec_t r = (pos - this->pos).length();
  if ( r < radius )
      r = radius;
  potential += qmod / r;
}

void StaticChargedSphere::draw(Screen& screen, const Screen::color& c)
{
  screen.set_pixel (pos, c);
}

std::string StaticChargedSphere::get_povray_script ()
{
  char buf[1024];
  sprintf (buf, "sphere { <%f, %f, %f>, %f texture {StaticLineTexture} }\n",
      prec2double (pos.x), prec2double (pos.y), prec2double (pos.z), prec2double (radius));
  return buf;
}

bool StaticChargedSphere::inContact (const vect3d& pos)
{
  return (pos - this->pos).magnitude() <= radiusradius;
}

StaticUniformSphere::StaticUniformSphere (const vect3d& pos, prec_t radius, prec_t q) : pos(pos), radius(radius), q(q)
{
  qmod = q * (SPEED_OF_LIGHT * SPEED_OF_LIGHT * MAGNETIC_CONSTANT);
  R2 = radius * radius;
  prec_t R3 = R2 * radius;
  imod = qmod / R3;
  rho = 3 / (4.0 * PREC_PI * radius * radius * radius);
}

StaticUniformSphere::~StaticUniformSphere ()
{
}

void StaticUniformSphere::addFields (const vect3d& pos, vect3d& bfield, vect3d& efield)
{
  vect3d r = pos - this->pos;
  prec_t rr = r * r;
  if ( rr >= R2 )
    {
      efield += (qmod / (rr * prec_t_sqrt(rr))) * r;
    }
  else if ( rr > 1e-23 )
    {
      efield += imod * r;
    }
}

void StaticUniformSphere::addPotential (const vect3d& pos, prec_t& potential)
{
  vect3d r = pos - this->pos;
  prec_t r2 = r * r;
  if ( r2 >= R2 )
    {
      prec_t rlen = prec_t_sqrt (r2);
      potential += qmod / rlen;
    }
  else
    {
      potential += qmod / radius;
      potential += qmod * 4.0 * PREC_PI * rho * (R2 - r2) / 6.0;
    }
}

void StaticUniformSphere::draw(Screen& screen, const Screen::color& c)
{
  screen.set_pixel (pos, c);
}

std::string StaticUniformSphere::get_povray_script ()
{
  char buf[1024];
  sprintf (buf, "sphere { <%f, %f, %f>, %f texture {StaticLineTexture} }\n",
      prec2double (pos.x), prec2double (pos.y), prec2double (pos.z), prec2double (radius));
  return buf;
}

bool StaticUniformSphere::inContact (const vect3d& pos)
{
  // never in contact
  return false;
}

StaticOctree::StaticOctree (Octree& tree, bool do_bfield, bool do_efield, bool do_pot, prec_t Bmul, prec_t Emul)
    : tree(tree), do_bfield(do_bfield), do_efield(do_efield), do_pot(do_pot), Bmul(Bmul), Emul(Emul)
{
}

StaticOctree::~StaticOctree ()
{
}

void StaticOctree::addFields (const vect3d& pos, vect3d& bfield, vect3d& efield)
{
  vect3d v[2];
  if ( !tree.getValues (pos, 0, v) )
      return;
  vect3d *vp = v;
  if ( do_bfield )
      bfield += Bmul * *vp++;
  if ( do_efield )
      efield += Emul * *vp;
}

void StaticOctree::addPotential (const vect3d& pos, prec_t& potential)
{
  if ( do_pot )
    {
      prec_t ret;
      if ( !tree.getValues (pos, &ret, 0) )
          return;
      potential += Emul * ret;
    }
}

void StaticOctree::draw(Screen& screen, const Screen::color& c)
{
}

std::string StaticOctree::get_povray_script ()
{
  return std::string();
}

bool StaticOctree::inContact (const vect3d& pos)
{
  return false;
}

StaticOctree24::StaticOctree24 (Octree& tree, bool do_bfield, bool do_efield, bool do_pot, prec_t Bmul, prec_t Emul)
    : StaticOctree (tree, do_bfield, do_efield, do_pot, Bmul, Emul)
{
}

StaticOctree24::~StaticOctree24 ()
{
}

void StaticOctree24::addFields (const vect3d& pos, vect3d& bfield, vect3d& efield)
{
  vect3d p = pos;
  const transf3d_pair& t = cube_polywell_transform24 (p);
  vect3d v[2];
  if ( !tree.getValues (p, 0, v) )
      return;
  vect3d *vp = v;
  if ( do_bfield )
      bfield += Bmul * (t.b * *vp++);
  if ( do_efield )
      efield += Emul * (t.b * *vp);
}

void StaticOctree24::addPotential (const vect3d& pos, prec_t& potential)
{
  if ( do_pot )
    {
      vect3d p = pos;
      cube_polywell_transform24 (p);
      prec_t ret;
      if ( !tree.getValues (p, &ret, 0) )
          return;
      potential += Emul * ret;
    }
}

StaticOctree48::StaticOctree48 (Octree& tree, bool do_bfield, bool do_efield, bool do_pot, prec_t Bmul, prec_t Emul)
    : StaticOctree (tree, do_bfield, do_efield, do_pot, Bmul, Emul)
{
}

StaticOctree48::~StaticOctree48 ()
{
}

void StaticOctree48::addFields (const vect3d& pos, vect3d& bfield, vect3d& efield)
{
  vect3d p = pos;
  const transf3d_pair& t = cube_polywell_transform48 (p);
  vect3d v[2];
  if ( !tree.getValues (p, 0, v) )
      return;
  vect3d *vp = v;
  if ( do_bfield )
      bfield += Bmul * (t.b * *vp++);
  if ( do_efield )
      efield += Emul * (t.b * *vp);
}

void StaticOctree48::addPotential (const vect3d& pos, prec_t& potential)
{
  if ( do_pot )
    {
      vect3d p = pos;
      cube_polywell_transform48 (p);
      prec_t ret;
      if ( !tree.getValues (p, &ret, 0) )
          return;
      potential += Emul * ret;
    }
}

Statics::Statics ()
{
  ambient_bfield.clear();
  ambient_efield.clear();
}

Statics::~Statics ()
{
  for ( size_t i = 0; i < strips.size(); i++ )
      delete strips[i];
}

void Statics::addStaticElement (StaticElement *strip)
{
  strips.push_back (strip);
}

void Statics::getFields (const vect3d& pos, vect3d& bfield, vect3d& efield)
{
  bfield = ambient_bfield;
  efield = ambient_efield;
  for ( size_t i = 0; i < strips.size(); i++ )
      strips[i]->addFields (pos, bfield, efield);
}

prec_t Statics::getPotential (const vect3d& pos)
{
  prec_t pot = 0;
  for ( size_t i = 0; i < strips.size(); i++ )
      strips[i]->addPotential (pos, pot);
  return pot;
}

bool Statics::inContact (const vect3d& pos)
{
  for ( size_t i = 0; i < strips.size(); i++ )
      if ( strips[i]->inContact(pos) )
          return true;
  return false;
}

void Statics::draw (Screen& screen, const Screen::color& c)
{
  for ( size_t i = 0; i < strips.size(); i++ )
      strips[i]->draw (screen, c);
}

std::string Statics::get_povray_script ()
{
  std::string ret;
  for ( size_t i = 0; i < strips.size(); i++ )
      ret += strips[i]->get_povray_script ();
  return ret;
}

std::string Statics::get_gnuplot_script ()
{
  std::string ret;
  for ( size_t i = 0; i < strips.size(); i++ )
      ret += strips[i]->get_gnuplot_script ();
  return ret;
}

prec_t Statics::get_ddsq ()
{
  return t_diff_ddsq;
}

void Statics::set_ddsq (prec_t ddsq)
{
  t_diff_ddsq = ddsq;
  small_increment = ddsq / 100.0;
}

